/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
# example client reading laser scans with swig wrapped c++ library in python
import sys, time, traceback
sys.path.append("../../../interfaces/wrap/Python/lib")
#sys.path.append("../../../interfaces/common")
#sys.path.append("../../../GIMI")
sys.path.append("../../../../GIMI/wrap/Python/lib")
import RangingClient
import gimi

def parseArgs(args):
	parsedArgs = {}
	nextIsValueOf = ""
	for arg in args:
		if not arg:
			continue
		if arg[0] == '-':
			nextIsValueOf = arg[1:]
			parsedArgs[nextIsValueOf] = ""
		else:
			if nextIsValueOf != "":
				parsedArgs[nextIsValueOf] = arg
				nextIsValueOf = ""
	return parsedArgs



def printUsage():
	print """
This is face detection based human detector from image data. It detects humans and updates them to database.
 
Usage is:	%s -i <saving interval in seconds> -R <Ranging source instance name> -f <filename to save into> [OPTIONS]
  [-u addr]			Use 'addr' as GIMnet AccessPoints address
  [-p port]			Use 'port' as GIMnet AccessPoints port
  [-n name]			Use 'name' as this nodes GIMnet name
  [-G name]			Specify the MaCIGroup for this module
  [-I name]			Specify the MaCIGroup Interface Instance
  
  [-v]				Increase verbosity by 1
  [-q]				Quiet mode
  [-h]				print this
  [-g]				use gui window 

  [-d time]			Specify duration of recording [s]

"""%sys.argv[0]
	

class rangingRecorder:
	"""detect humans from faces in images and update them to db"""
	def __init__(self):
		#defaults
		self.name = ""
		self.groupName = "hinkuvinku"
		self.instanceName = "nicolas"
		self.server = "asrobo.hut.fi"
		self.port = 40002
		self.rangingSource = "Base"
		self.interval = 0.5#[s]
		self.filename = ""
		self.gui = False
		self.bRunning = True
		self.duration = None
		self.beginTime = time.time()

		if len(sys.argv) < 4:
			printUsage()
		#	sys.exit()
		
		args = parseArgs(sys.argv)
		#print args
		

		if args.has_key("h"):
			printUsage()
			sys.exit()
		
		print "Ranging recorder"
		if args.has_key("u"):self.server = args['u']
		if args.has_key("p"):self.port = int(args['p'])
		if args.has_key("n"):self.name = args['n']
		if args.has_key("G"):self.groupName = args['G']
		if args.has_key("I"):self.instanceName = args['I']
		if args.has_key("i"):self.interval = float(args['i'])
		if args.has_key("R"):self.rangingSource = args['R']
		if args.has_key("f"):self.filename = args['f']
		if args.has_key("g"):self.gui = True
		if args.has_key("d"):self.duration = int(args['d'])
				
		if self.interval < 0:
			print "ERROR: interval cant be negative!"
			sys.exit()
			
		self.g = gimi.GIMI()
		self.rc = RangingClient.CRangingClient(self.g, 0)
		
		print "Connecting to %s:%s as %s..."%(self.server, self.port, self.name),
		ret = self.g.connectToHubEx(self.server, self.port, self.name)
		if ret != RangingClient.KMaCIOK:
			print "Failed: ", RangingClient.GetErrorStr(ret)
			sys.exit()
		else: print RangingClient.GetErrorStr(ret)
		
		target = "%s.MaCI_Ranging.%s"%(self.groupName, self.rangingSource)
		print "setting target: %s"%target
		self.rc.SetDefaultTarget(RangingClient.SMaCISL(target))
		
		
		print "open Ranging...",
		ret = self.rc.Open()
		if ret != RangingClient.KMaCIOK:
			print "Failed: ", RangingClient.GetErrorStr(ret)
			self.rc.Close()
			sys.exit()
		else: print RangingClient.GetErrorStr(ret)
		
		print "opening file %s..."%self.filename,
		self.logFile = open(self.filename, 'w')
		time.sleep(1)
		

	def recordingLoop(self):
#		if self.gui:
#			detectHumanFace.cvStartWindowThread()
#			detectHumanFace.cvNamedWindow("The Faces", 0)

		#construct header
		tStamp = RangingClient.TTimestamp()
		index = -1
		header = RangingClient.TDistanceHeader()
		array = RangingClient.TDistanceArray()
		ret = self.rc.GetDistanceArray(array, header, tStamp, index, 1000)
		if ret.__len__() == 3:
			firstLine = "count width startAngle max min x y z heading\n %i %f %f"%(header.beam_count, header.beam_width, array[0].angle)
			
		limits = RangingClient.TDeviceInformationLimits()
		ret = self.rc.GetDeviceLimits(limits, 1000)
		if ret:
			firstLine += " %f %f"%(limits.max_distance, limits.min_distance)

		pose = RangingClient.TDeviceInformationPosition()
		ret = self.rc.GetDevicePosition(pose, 1000)
		if ret:
			firstLine += " %f %f %f %f"%(pose.x, pose.y, pose.z, pose.heading)
		firstLine += "\n"	
		print firstLine
		self.logFile.write(firstLine)
		secondLine = "timestamp[s] scan scan scan ..."
		self.logFile.write(secondLine)
		self.beginTime = time.time()
		try:
			while True:
				startTime = time.time()
				ret = self.rc.GetDistanceArray(array, header, tStamp, index, 1000)
				if ret.__len__() == 3:
					line = "%i.%.6i"%(tStamp.timestamp_s, tStamp.timestamp_us)
					for i in range(header.beam_count):
						line += " %f"%array[i].distance
					line += "\n"
					self.logFile.write(line)
					
					sleepTime = self.interval - (time.time() - startTime)
					if sleepTime > 0.0:
						time.sleep(sleepTime)
				if self.duration:
					if (startTime - self.beginTime) > self.duration:
						raise NameError('Recording time finished')
					
		except:
			print "detectionLoop exited"
			if self.gui:
				detectHumanFace.cvDestroyWindow("The Faces")
			traceback.print_exc()
			self.logFile.close()
			self.rc.Close()
			self.g.stop()

def main():
	recorder = rangingRecorder()
	recorder.recordingLoop()
	
if __name__ == "__main__":
	main()
	
	
